#include "V4l2Camera.h"

Image::Image(int width, int height,int depth){
    Myformat.width = width;
    Myformat.height = height;
    Myformat.depth = depth;
}

Image::Image(char* filename){
    Myimread(filename);
}

bool Image::Init(uint8_t* head){
    if(Inited){
        perror("Already inited!\n");
        exit(EXIT_FAILURE);
    }
    
    Myhead.reset(head);
    MybyteUsed = Myformat.depth * Myformat.height * Myformat.depth;
    Inited = true;
    return true;
}

uint8_t Image::at(int w, int h, int c){
    if(!Inited){
        perror("This image you are going to use have not been inited!\n");
        exit(EXIT_FAILURE);
    }
    return Myhead[(w*Myformat.width + h) * Myformat.depth + c - 1];
}

void Image::setAt(int w, int h, int c, int value){
    if(!Inited){
        perror("This image you are going to use have not been inited!\n");
        exit(EXIT_FAILURE);
    }
    Myhead[(w*Myformat.width + h) * Myformat.depth + c - 1] = value;
    return;
}

void Image::drawRec(int x1, int y1, int x2, int y2){
    for(int i = y1;i < y2;i++){
        setAt(x1, i, 1, 0);  setAt(x2, i, 1, 0);
        setAt(x1, i, 2, 255);setAt(x2, i, 2, 255);
        setAt(x1, i, 3, 0);  setAt(x2, i, 3, 0);
    }
    for(int i = x1;i < x2;i++){
        setAt(i, y1, 1, 0);  setAt(i, y2, 1, 0);
        setAt(i, y1, 2, 255);setAt(i, y2, 2, 255);
        setAt(i, y1, 3, 0);  setAt(i, y2, 3, 0);
    }
}

bool Image::saveMe(const char* filename, int quality) const{
    if(!Inited){
        char thisError [100];
        sscanf(thisError, "%s you are going to save have not been inited!\n", filename);
        perror(thisError);
        exit(EXIT_FAILURE);
    }
    unsigned char buf = 0;
	struct jpeg_compress_struct cinfo;
	struct jpeg_error_mgr jerr;
	FILE* outfile;                 //target file 
	JSAMPROW row_pointer[1];        //pointer to JSAMPLE row[s] 
	int     row_stride;             //physical row width in image buffer 
	cinfo.err = jpeg_std_error(&jerr);
	jpeg_create_compress(&cinfo);

	outfile = fopen(filename, "wb");
	if (NULL == outfile)
	{
		char thisError [100];
        sscanf(thisError, "Failed to open %s!\n", filename);
        perror(thisError);
		exit(EXIT_FAILURE);
	}
	jpeg_stdio_dest(&cinfo, outfile);
	cinfo.image_width = Myformat.width;
	cinfo.image_height = Myformat.height;
	cinfo.input_components = 3;        
	cinfo.in_color_space = JCS_RGB;

	jpeg_set_defaults(&cinfo);
	jpeg_set_quality(&cinfo, quality, TRUE);
	jpeg_start_compress(&cinfo, TRUE);

	row_stride = Myformat.width * Myformat.depth; 
	while (cinfo.next_scanline < cinfo.image_height)
	{
		//row_pointer[0] = & bits[cinfo.next_scanline * row_stride];
		//row_pointer[0] = &Myhead[(cinfo.image_height - cinfo.next_scanline - 1) * row_stride];
        row_pointer[0] = &Myhead[cinfo.next_scanline * row_stride];
		(void)jpeg_write_scanlines(&cinfo, row_pointer, 1);
	}
	jpeg_finish_compress(&cinfo);
	fclose(outfile);
	jpeg_destroy_compress(&cinfo);
	return true;
}

bool Image::Myimread(char *filename) {
    FILE *infile = fopen(filename, "rb");
    if (!infile) {
        char thisError [100];
        sscanf(thisError, "%s you are going to save have not been inited!\n", filename);
        perror(thisError);
        exit(EXIT_FAILURE);
    }

    struct jpeg_decompress_struct cinfo;
    struct jpeg_error_mgr jerr;
    cinfo.err = jpeg_std_error(&jerr);
    jpeg_create_decompress(&cinfo);

    jpeg_stdio_src(&cinfo, infile);
    jpeg_read_header(&cinfo, TRUE);
    jpeg_start_decompress(&cinfo);

    Myformat.width = cinfo.output_width;
    Myformat.height = cinfo.output_height;
    Myformat.depth = cinfo.output_components;
    printf("%s:\n\tWidth: %d, Height: %d, Channels: %d\n",filename, Myformat.width, Myformat.height, Myformat.depth);

    uint8_t* buffer = (uint8_t*)malloc(Myformat.width * Myformat.height * Myformat.depth);

    while (cinfo.output_scanline < Myformat.height) {
        uint8_t* row_pointer = buffer + cinfo.output_scanline * Myformat.width * Myformat.depth;
        jpeg_read_scanlines(&cinfo, &row_pointer, 1);
    }

    jpeg_finish_decompress(&cinfo);
    jpeg_destroy_decompress(&cinfo);

    fclose(infile);

    Init(buffer);
    return true;
}

// *******************************************************
//                          VCAM
// *******************************************************
Vcam::Vcam(char* device_name, int width, int height, int fps){
    Mywidth = width;
    Myheight = height;
    Myfps = fps;
    sscanf(device_name+10, "%d", &Mydevice);
    Myfd = open(device_name, O_RDWR);

    if(Myfd < 0){
        perror("Video open failed\n");
        exit(EXIT_FAILURE);
    }
    else{
        printf("Video open success!\nDevice Node is: %d\n", Mydevice);
    }
}

Vcam::~Vcam(){
}

bool Vcam::Init(){
    if(Inited){
        perror("Already inited!\n");
        exit(EXIT_FAILURE);
    }

    struct v4l2_format vfmt;
    vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    vfmt.fmt.pix.width = Mywidth;
    vfmt.fmt.pix.height = Myheight;
    vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
    
    if(ioctl(Myfd,VIDIOC_S_FMT,&vfmt) < 0){
        perror("Set format failed.\n");
        exit(EXIT_FAILURE);
    }
    // 检查设置参数是否生效
    if(ioctl(Myfd,VIDIOC_G_FMT,&vfmt) < 0){
        perror("Get format info failed.\n");
        exit(EXIT_FAILURE);
    }
    else if(vfmt.fmt.pix.width == Mywidth && vfmt.fmt.pix.height == Myheight && vfmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG){
        printf("Setting format,the actually wh is <%d * %d>,img format:Motion-JPEG\n",vfmt.fmt.pix.width, vfmt.fmt.pix.height);
    }
    else{
        printf("Setting format,the actually wh is <%d * %d>,img format:Motion-JPEG\n",vfmt.fmt.pix.width, vfmt.fmt.pix.height);
        perror("Setted format did not work.\n");
        exit(EXIT_FAILURE);
    }
    struct v4l2_requestbuffers reqbuf;
    reqbuf.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuf.count = 3;   //3个帧缓冲
    reqbuf.memory = V4L2_MEMORY_MMAP;
    if(ioctl(Myfd,VIDIOC_REQBUFS,&reqbuf) < 0){
        perror("Requiest for buffer failed.\n");
        exit(EXIT_FAILURE);
    }

    // 将帧缓冲映射到进程地址空间
    struct v4l2_buffer buf;
    buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    // 将每一帧对应的缓冲区的起始地址保存在frm_base数组中，读取采集数据时，只需直接读取映射区
    for(buf.index=0;buf.index<3;buf.index++){
        ioctl(Myfd, VIDIOC_QUERYBUF, &buf);
        Myfrm_base[buf.index] = mmap(NULL,buf.length,PROT_READ | PROT_WRITE,MAP_SHARED,Myfd,buf.m.offset);
        Myfrm_size[buf.index] = buf.length;

        if(Myfrm_base[buf.index] == MAP_FAILED){
            perror("mmap failed\n");
            exit(EXIT_FAILURE);
        }

        // 入队操作
        if(ioctl(Myfd,VIDIOC_QBUF,&buf) < 0){
            perror("Push in queue failed.\n");
            exit(EXIT_FAILURE);
        }
    }

    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(Myfd, VIDIOC_STREAMON, &type) < 0){
        perror("Captureing failed.\n");
        exit(EXIT_FAILURE);
    }

    Myreadbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    Myreadbuffer.memory = V4L2_MEMORY_MMAP;
    if(ioctl(Myfd, VIDIOC_DQBUF, &Myreadbuffer) < 0){
        perror("Reading frame failed.\n");
        exit(EXIT_FAILURE);
    }

    Inited = true;
    return true;
}

void Vcam::printCamInfo(){
    if(!Inited){
        perror("This cam you are going to use have not been inited!\n");
        exit(EXIT_FAILURE);
    }

    struct v4l2_fmtdesc fmtdesc;
    fmtdesc.index = 0;
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    
    printf("CAM supported all formats are like:\n");
    while(ioctl(Myfd,VIDIOC_ENUM_FMT,&fmtdesc) == 0){
        printf("v4l2_format%d:%s\n",fmtdesc.index,fmtdesc.description);
        fmtdesc.index++;
    }

    struct v4l2_frmsizeenum frmsize;
    frmsize.index = 0;
    frmsize.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    printf("MJPEG format supported format are like:\n");
    frmsize.pixel_format = V4L2_PIX_FMT_MJPEG;
    while(ioctl(Myfd,VIDIOC_ENUM_FRAMESIZES,&frmsize) == 0){
        printf("frame_size<%d*%d>\n",frmsize.discrete.width,frmsize.discrete.height);
        frmsize.index++;
    }

    struct v4l2_frmivalenum frmival;
    frmival.index = Mydevice;
    frmival.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    frmival.pixel_format = V4L2_PIX_FMT_MJPEG;
    frmival.width = Mywidth;
    frmival.height = Myheight;

    while(ioctl(Myfd,VIDIOC_ENUM_FRAMEINTERVALS,&frmival) == 0){
        printf("frame_interval under frame_size <%d*%d> support %dfps\n",frmival.width,frmival.height,frmival.discrete.denominator / frmival.discrete.numerator);
        frmival.index++;
    }

    return;
}

Image* Vcam::getNextFrame(){
    if(!Inited){
        perror("This cam you are going to use have not been inited!\n");
        exit(EXIT_FAILURE);
    }

    if(ioctl(Myfd, VIDIOC_DQBUF, &Myreadbuffer) < 0){
        perror("Reading frame failed.\n");
    }

    //  这一帧
    if(Myfrm_base[Myreadbuffer.index] == MAP_FAILED){
        perror("While Capturing, found MMapfiled.\n");
        exit(EXIT_FAILURE);
    }

    Image* thisImage = decodeMjpeg((uint8_t*)Myfrm_base[Myreadbuffer.index], Myreadbuffer.bytesused);

    if(ioctl(Myfd,VIDIOC_QBUF,&Myreadbuffer) < 0){
        perror("Failing enqueue.\n");
    }

    return thisImage;
}


Image* Vcam::decodeMjpeg(uint8_t *mjpeg_data, unsigned long mjpeg_size){
    struct jpeg_decompress_struct cinfo;
    struct jpeg_error_mgr jerr;
    uint8_t* rgb_buffer = NULL;

    cinfo.err = jpeg_std_error(&jerr);
    jpeg_create_decompress(&cinfo);

    jpeg_mem_src(&cinfo, mjpeg_data, mjpeg_size);
    jpeg_read_header(&cinfo, TRUE);
    jpeg_start_decompress(&cinfo);

    //printf("Frame:\n\tWidth: %d, Height: %d, Channels: %d\n",cinfo.output_width, cinfo.output_height, cinfo.output_components);
    rgb_buffer = (uint8_t *)malloc(Mywidth * Myheight * 3);

    while (cinfo.output_scanline < cinfo.output_height) {
        uint8_t* row_pointer = rgb_buffer + cinfo.output_scanline * Mywidth * 3;
        jpeg_read_scanlines(&cinfo, &row_pointer, 1);
    }

    jpeg_finish_decompress(&cinfo);
    jpeg_destroy_decompress(&cinfo);

    Image* thisImage = new Image(Mywidth, Myheight, 3);
    thisImage->Init(rgb_buffer);
    return thisImage;
}